% Computes optimal solution for half of a periodical cycle for a 5-link humanoid robot  
% model: five revolute joints liked by five rigid bodies that represent
% legs ( tibia + thigh) and upper body.
% Obs: It is necessary to have the TOMLAB toolbox PROPT for advanced optimal control applications installed in MATLAB.
% Last modification: 30/03/2014 (by Feliphe G. Galiza)

close all
clear 
clc

% parameters according to human body
height = 1.5; % [m]
L1 = 0.285*height; % [m]
L2 = (0.53 - 0.285)*height; % [m]
L3 = height - (L1 + L2); % [m]
L4 = L2; % [m]
L5 =L1; % [m]

r1 = L1/2; % [m]
r2 = L2/2; % [m]
r3 = L3/2; % [m]    
r4 = L4/2; % [m]
r5 = L5/2; % [m]

mass = 50; % [kg]
m1 = 0.061 * mass; % [kg]
m2 = 0.1 * mass; % [kg]
m3 = 0.678 * mass; % [kg]
m4 = m2; % [kg]
m5 = m1; % [kg]
m = [m1 m2 m3 m4 m5] ;
Izz_1 = m1 * 0.735^2; % [kg*m^2]
Izz_2 = m2 * 0.540^2; % [kg*m^2]
Izz_3 = m3 * 0.0798^2; % [kg*m^2]
Izz_4 = Izz_2; % [kg*m^2]
Izz_5 = Izz_1; % [kg*m^2]

Izz = [ Izz_1 Izz_2 Izz_3 Izz_4 Izz_5];

g =9.81; %[m/s^2]

%PROPT

toms t tf
nn = 30; % # nodes
p = tomPhase('p',t,0,tf,nn);
setPhase(p)

tomStates x1 x2 x3 x4 x5 x6 x7 x8 x9 x10
tomControls u1 u2 u3 u4 u5

% State Space Representantion
% where: 
% x1_dot = theta1_dot = x6
% x2_dot = theta2_dot = x7
% x3_dot = theta3_dot = x8
% x4_dot = theta4_dot = x9
% x5_dot = theta5_dot = x10
% x6_dot = theta_acc1
% x7_dot = theta_acc2
% x8_dot = theta_acc3
% x9_dot = theta_acc4
% x10_dot = theta_acc5

q = [x1; x2; x3; x4; x5]; % joint angles
qd = [x6; x7; x8; x9; x10]; % joint velocities
u = [u1; u2; u3; u4; u5]; % joint controls a.k.a. joint torques

% Initial guess for states
x0 = {tf == 2
    icollocate({
     x1 == -0.1894          
     x2 == 0
     x3 == 0
     x4 == -0.7227
     x5 == 0
    x6 == 0
    x7 == 0
    x8 == 0
    x9 == 0
    x10 == 0})};

% Initial guess for states
x0 = {x0
    collocate({
    u1 == 0
    u2 == 0
    u3 == 0
    u4 == 0
    u5 == 0})};


% Box constraints
% 1 ft = 0.3 metros
d1=0.05; % forefoot size in [m]
d2=0.25; % heel size in [m]
L = d1 + d2; % foot size in [m]

% pm  is a 2 x 5 array with the joint cartesian position of each body 
% pcm is a 2 x 5 array with the Center of Mass position of each body  
[pm, pcm] = computeFK(q);

% state space representation
theta1 = x1;
theta2 = x2;
theta3= x3;
theta4 = x4;
theta5 = x5;

theta1_dot = x6;
theta2_dot = x7;
theta3_dot = x8;
theta4_dot  = x9;
theta5_dot = x10;

% Computing ZMP 

% jacobian
J=[ 
                                                                                                                                   cos(theta1)*(L1 - r1),                                                                                                                               0,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                                                  -sin(theta1)*(L1 - r1),                                                                                                                               0,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                  cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1),                                                                                                  cos(theta1 + theta2)*(L2 - r2),                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1),                                                                                                 -sin(theta1 + theta2)*(L2 - r2),                                                                                                0,                                                                                                0,                                                   0; ...
                                                               cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) + r3*cos(theta1 + theta2 + theta3),                                                               cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3),                                                                 r3*cos(theta1 + theta2 + theta3),                                                                                                0,                                                   0; ...
                                                             - r3*sin(theta1 + theta2 + theta3) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1),                                                             - r3*sin(theta1 + theta2 + theta3) - sin(theta1 + theta2)*(L2 - r2),                                                                -r3*sin(theta1 + theta2 + theta3),                                                                                                0,                                                   0; ...
                                                      cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) - r4*cos(theta1 + theta2 + theta3 - theta4),                                                      cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4),                                                       -r4*cos(theta1 + theta2 + theta3 - theta4),                                                        r4*cos(theta1 + theta2 + theta3 - theta4),                                                   0; ...
                                                      r4*sin(theta1 + theta2 + theta3 - theta4) - sin(theta1)*(L1 - r1) - sin(theta1 + theta2)*(L2 - r2),                                                      r4*sin(theta1 + theta2 + theta3 - theta4) - sin(theta1 + theta2)*(L2 - r2),                                                        r4*sin(theta1 + theta2 + theta3 - theta4),                                                       -r4*sin(theta1 + theta2 + theta3 - theta4),                                                   0; ...
 cos(theta1 + theta2)*(L2 - r2) - r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + cos(theta1)*(L1 - r1) - r4*cos(theta1 + theta2 + theta3 - theta4), cos(theta1 + theta2)*(L2 - r2) - r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - r4*cos(theta1 + theta2 + theta3 - theta4), - r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - r4*cos(theta1 + theta2 + theta3 - theta4),   r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4), -r5*cos(theta1 + theta2 + theta3 - theta4 + theta5); ...
 r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1) + r4*sin(theta1 + theta2 + theta3 - theta4), r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4),   r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4), - r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - r4*sin(theta1 + theta2 + theta3 - theta4),  r5*sin(theta1 + theta2 + theta3 - theta4 + theta5); ...
                                                                                                                                                      -1,                                                                                                                               0,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                                0,                                                                                                0,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                               -1,                                                                                                0,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                               -1,                                                                                                1,                                                   0; ...
                                                                                                                                                      -1,                                                                                                                              -1,                                                                                               -1,                                                                                                1,                                                  -1];
 

% jacobian derivative
Jd =[                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               -theta1_dot*sin(theta1)*(L1 - r1),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0; ...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               -theta1_dot*cos(theta1)*(L1 - r1),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               - theta1_dot*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1)) - theta2_dot*sin(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 - theta1_dot*sin(theta1 + theta2)*(L2 - r2) - theta2_dot*sin(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1)) - theta2_dot*cos(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 - theta1_dot*cos(theta1 + theta2)*(L2 - r2) - theta2_dot*cos(theta1 + theta2)*(L2 - r2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1)) - theta2_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2)) - r3*theta3_dot*sin(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2)) - theta2_dot*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2)) - r3*theta3_dot*sin(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                             - r3*theta1_dot*sin(theta1 + theta2 + theta3) - r3*theta2_dot*sin(theta1 + theta2 + theta3) - r3*theta3_dot*sin(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) + r3*cos(theta1 + theta2 + theta3)) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3)) - r3*theta3_dot*cos(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                         - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3)) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) + r3*cos(theta1 + theta2 + theta3)) - r3*theta3_dot*cos(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                             - r3*theta1_dot*cos(theta1 + theta2 + theta3) - r3*theta2_dot*cos(theta1 + theta2 + theta3) - r3*theta3_dot*cos(theta1 + theta2 + theta3),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                         r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - theta2_dot*(sin(theta1 + theta2)*(L2 - r2) - r4*sin(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1) - r4*sin(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                         r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - theta2_dot*(sin(theta1 + theta2)*(L2 - r2) - r4*sin(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(sin(theta1 + theta2)*(L2 - r2) - r4*sin(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta1_dot*sin(theta1 + theta2 + theta3 - theta4) + r4*theta2_dot*sin(theta1 + theta2 + theta3 - theta4) + r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta4_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta2_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta3_dot*sin(theta1 + theta2 + theta3 - theta4) - r4*theta1_dot*sin(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                         r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) - r4*cos(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                         r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - theta2_dot*(cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(cos(theta1 + theta2)*(L2 - r2) - r4*cos(theta1 + theta2 + theta3 - theta4)) - r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta1_dot*cos(theta1 + theta2 + theta3 - theta4) + r4*theta2_dot*cos(theta1 + theta2 + theta3 - theta4) + r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                             r4*theta4_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta2_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta3_dot*cos(theta1 + theta2 + theta3 - theta4) - r4*theta1_dot*cos(theta1 + theta2 + theta3 - theta4),                                                                                                                                                                                                                                                                                                                             0;...
 theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), theta4_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta2_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta3_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r4*sin(theta1 + theta2 + theta3 - theta4)) - r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5), r5*theta1_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta2_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta3_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) - r5*theta4_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta5_dot*sin(theta1 + theta2 + theta3 - theta4 + theta5);...
 theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) - cos(theta1)*(L1 - r1) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) + r4*cos(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) + r4*cos(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), theta4_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta2_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta3_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - theta1_dot*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r4*cos(theta1 + theta2 + theta3 - theta4)) - r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5), r5*theta1_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta2_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta3_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) - r5*theta4_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5) + r5*theta5_dot*cos(theta1 + theta2 + theta3 - theta4 + theta5);...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0;...
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                             0];



vel = J*qd; % absolut velocity of each body

angvel= [vel(11,1);vel(12,1);vel(13,1);vel(14,1);vel(15,1)]; % absolut angular velocity of each body

acc = J*dot(qd) +Jd*qd; % absolute acceleration of each body

x_acc = [acc(1); acc(3); acc(5); acc(7); acc(9)]; % acceleration in x direction
y_acc = [acc(2); acc(4); acc(6); acc(8); acc(10)]; % acceleration in y direction

 xZMP_num = 0;
 xZMP_den = 0;
 for i=1:5
     xZMP_num = xZMP_num + m(i)*(y_acc(i)+g)*pcm(1,i) - m(i)*x_acc(i)*pcm(2,i) - Izz(i)*angvel(i);
     xZMP_den = xZMP_den + m(i)*(y_acc(i)+g);
 end
 xZMP = xZMP_num/xZMP_den;

%xCOM = computeCM(pcm(1,:));

%xZMP = computeCM(pcm(1,:)) - x_acc*computeCM(pcm(2,:))/g;  

cbox = { 0.1 <= tf <= 10
    -pi <= icollocate(x1) <= pi
    -pi <= icollocate(x3) <= pi
    -pi <= icollocate(x4) <= pi
    -pi <= icollocate(x2) <= 0
     0 <= icollocate(x5) <= pi
    -10*pi <= icollocate(qd) <= 10*pi
    -d1 <= icollocate(xZMP) <= d2
    -200 <= collocate(u1) <= 200
    -200 <= collocate(u2) <= 200 
    -200 <= collocate(u3) <= 200 
    -200 <= collocate(u4) <= 200 
    -200 <= collocate(u5) <= 200};

% 5th-link foot position
x_m5 = pm(1,5);
y_m5 = pm(2,5);

% Boundary constraints
cbnd = {final(x5) == initial(x2)
    final(x2) == initial(x5) 
    final(x4) ==  initial(x3) 
    final(x3) ==  initial(x4)
    final(x1) == (initial(x1) + initial(x2) + initial(x3) - initial(x4) + initial(x5))
    initial(x1) == (final(x1) + final(x2) + final(x3) - final(x4) + final(x5))
    initial(x_m5) == -L
    initial(y_m5) == 0
    initial(xZMP) == -d1
    final(xZMP) == d2
    final(x10) == initial(x7)
    final(x7) == initial(x10)
    final(x9) == -initial(x8) 
    final(x8) == -initial(x9) 
    final(x6) == initial(x6) + initial(x7) + initial(x8) - initial(x9) + initial(x10)
    initial(x6) == (final(x6) + final(x7) + final(x8) - final(x9) + final(x10))
    initial(x6) == 0
    final(x10) == 0};


% Equation of Motion in the form M*dot(qd) = RHS
% Using Newton-Euler
M(1,:) = [ Izz_1 + Izz_2 + Izz_3 + Izz_4 + Izz_5 + m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1) + r4*sin(theta1 + theta2 + theta3 - theta4))^2 + m4*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) - r4*cos(theta1 + theta2 + theta3 - theta4))^2 + m3*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1) + r3*cos(theta1 + theta2 + theta3))^2 + m4*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1) - r4*sin(theta1 + theta2 + theta3 - theta4))^2 + m5*(r5*cos(theta1 + theta2 + theta3 - theta4 + theta5) - cos(theta1 + theta2)*(L2 - r2) - cos(theta1)*(L1 - r1) + r4*cos(theta1 + theta2 + theta3 - theta4))^2 + m2*(cos(theta1 + theta2)*(L2 - r2) + cos(theta1)*(L1 - r1))^2 + m2*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1))^2 + m3*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1))^2 + m1*cos(theta1)^2*(L1 - r1)^2 + m1*sin(theta1)^2*(L1 - r1)^2, Izz_2 + Izz_3 + Izz_4 + Izz_5 + L2^2*m2 + L2^2*m3 + L2^2*m4 + L2^2*m5 + m2*r2^2 + m3*r2^2 + m3*r3^2 + m4*r2^2 + m5*r2^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - 2*L2*m2*r2 - 2*L2*m3*r2 - 2*L2*m4*r2 - 2*L2*m5*r2 - L1*m4*r4*cos(theta2 + theta3 - theta4) - L1*m5*r4*cos(theta2 + theta3 - theta4) - 2*L2*m5*r5*cos(theta3 - theta4 + theta5) + m4*r1*r4*cos(theta2 + theta3 - theta4) + m5*r1*r4*cos(theta2 + theta3 - theta4) + 2*m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m3*r3*cos(theta2 + theta3) + L1*L2*m2*cos(theta2) + L1*L2*m3*cos(theta2) + L1*L2*m4*cos(theta2) + L1*L2*m5*cos(theta2) - m3*r1*r3*cos(theta2 + theta3) - L1*m2*r2*cos(theta2) - L2*m2*r1*cos(theta2) - L1*m3*r2*cos(theta2) - L2*m3*r1*cos(theta2) - L1*m4*r2*cos(theta2) - L2*m4*r1*cos(theta2) - L1*m5*r2*cos(theta2) - L2*m5*r1*cos(theta2) + 2*L2*m3*r3*cos(theta3) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) + m2*r1*r2*cos(theta2) + m3*r1*r2*cos(theta2) + m4*r1*r2*cos(theta2) + m5*r1*r2*cos(theta2) - 2*m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - 2*L2*m4*r4*cos(theta3 - theta4) - 2*L2*m5*r4*cos(theta3 - theta4) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) + 2*m4*r2*r4*cos(theta3 - theta4) + 2*m5*r2*r4*cos(theta3 - theta4), Izz_3 + Izz_4 + Izz_5 + m3*r3^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - L1*m4*r4*cos(theta2 + theta3 - theta4) - L1*m5*r4*cos(theta2 + theta3 - theta4) - L2*m5*r5*cos(theta3 - theta4 + theta5) + m4*r1*r4*cos(theta2 + theta3 - theta4) + m5*r1*r4*cos(theta2 + theta3 - theta4) + m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m3*r3*cos(theta2 + theta3) - m3*r1*r3*cos(theta2 + theta3) + L2*m3*r3*cos(theta3) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) - m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - L2*m4*r4*cos(theta3 - theta4) - L2*m5*r4*cos(theta3 - theta4) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) + m4*r2*r4*cos(theta3 - theta4) + m5*r2*r4*cos(theta3 - theta4), L1*m4*r4*cos(theta2 + theta3 - theta4) - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - Izz_4 + L1*m5*r4*cos(theta2 + theta3 - theta4) + L2*m5*r5*cos(theta3 - theta4 + theta5) - m4*r1*r4*cos(theta2 + theta3 - theta4) - m5*r1*r4*cos(theta2 + theta3 - theta4) - m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) - 2*m5*r4*r5*cos(theta5) + L2*m4*r4*cos(theta3 - theta4) + L2*m5*r4*cos(theta3 - theta4) - m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) - m4*r2*r4*cos(theta3 - theta4) - m5*r2*r4*cos(theta3 - theta4), Izz_5 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) + m5*r4*r5*cos(theta5) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5)];
M(2,:) = [ Izz_2 + Izz_3 + Izz_4 + Izz_5 + L2^2*m2 + L2^2*m3 + L2^2*m4 + L2^2*m5 + m2*r2^2 + m3*r2^2 + m3*r3^2 + m4*r2^2 + m5*r2^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - 2*L2*m2*r2 - 2*L2*m3*r2 - 2*L2*m4*r2 - 2*L2*m5*r2 - L1*m4*r4*cos(theta2 + theta3 - theta4) - L1*m5*r4*cos(theta2 + theta3 - theta4) - 2*L2*m5*r5*cos(theta3 - theta4 + theta5) + m4*r1*r4*cos(theta2 + theta3 - theta4) + m5*r1*r4*cos(theta2 + theta3 - theta4) + 2*m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m3*r3*cos(theta2 + theta3) + L1*L2*m2*cos(theta2) + L1*L2*m3*cos(theta2) + L1*L2*m4*cos(theta2) + L1*L2*m5*cos(theta2) - m3*r1*r3*cos(theta2 + theta3) - L1*m2*r2*cos(theta2) - L2*m2*r1*cos(theta2) - L1*m3*r2*cos(theta2) - L2*m3*r1*cos(theta2) - L1*m4*r2*cos(theta2) - L2*m4*r1*cos(theta2) - L1*m5*r2*cos(theta2) - L2*m5*r1*cos(theta2) + 2*L2*m3*r3*cos(theta3) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) + m2*r1*r2*cos(theta2) + m3*r1*r2*cos(theta2) + m4*r1*r2*cos(theta2) + m5*r1*r2*cos(theta2) - 2*m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - 2*L2*m4*r4*cos(theta3 - theta4) - 2*L2*m5*r4*cos(theta3 - theta4) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) + 2*m4*r2*r4*cos(theta3 - theta4) + 2*m5*r2*r4*cos(theta3 - theta4), Izz_2 + Izz_3 + Izz_4 + Izz_5 + L2^2*m2 + L2^2*m3 + L2^2*m4 + L2^2*m5 + m2*r2^2 + m3*r2^2 + m3*r3^2 + m4*r2^2 + m5*r2^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - 2*L2*m2*r2 - 2*L2*m3*r2 - 2*L2*m4*r2 - 2*L2*m5*r2 - 2*L2*m5*r5*cos(theta3 - theta4 + theta5) + 2*m5*r2*r5*cos(theta3 - theta4 + theta5) + 2*L2*m3*r3*cos(theta3) - 2*m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - 2*L2*m4*r4*cos(theta3 - theta4) - 2*L2*m5*r4*cos(theta3 - theta4) + 2*m4*r2*r4*cos(theta3 - theta4) + 2*m5*r2*r4*cos(theta3 - theta4), Izz_3 + Izz_4 + Izz_5 + m3*r3^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) + L2*m3*r3*cos(theta3) - m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - L2*m4*r4*cos(theta3 - theta4) - L2*m5*r4*cos(theta3 - theta4) + m4*r2*r4*cos(theta3 - theta4) + m5*r2*r4*cos(theta3 - theta4), L2*m5*r5*cos(theta3 - theta4 + theta5) - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - Izz_4 - m5*r2*r5*cos(theta3 - theta4 + theta5) - 2*m5*r4*r5*cos(theta5) + L2*m4*r4*cos(theta3 - theta4) + L2*m5*r4*cos(theta3 - theta4) - m4*r2*r4*cos(theta3 - theta4) - m5*r2*r4*cos(theta3 - theta4), Izz_5 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) + m5*r4*r5*cos(theta5)];
M(3,:) = [ Izz_3 + Izz_4 + Izz_5 + m3*r3^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - L1*m4*r4*cos(theta2 + theta3 - theta4) - L1*m5*r4*cos(theta2 + theta3 - theta4) - L2*m5*r5*cos(theta3 - theta4 + theta5) + m4*r1*r4*cos(theta2 + theta3 - theta4) + m5*r1*r4*cos(theta2 + theta3 - theta4) + m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m3*r3*cos(theta2 + theta3) - m3*r1*r3*cos(theta2 + theta3) + L2*m3*r3*cos(theta3) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) - m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - L2*m4*r4*cos(theta3 - theta4) - L2*m5*r4*cos(theta3 - theta4) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) + m4*r2*r4*cos(theta3 - theta4) + m5*r2*r4*cos(theta3 - theta4), Izz_3 + Izz_4 + Izz_5 + m3*r3^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) + L2*m3*r3*cos(theta3) - m3*r2*r3*cos(theta3) + 2*m5*r4*r5*cos(theta5) - L2*m4*r4*cos(theta3 - theta4) - L2*m5*r4*cos(theta3 - theta4) + m4*r2*r4*cos(theta3 - theta4) + m5*r2*r4*cos(theta3 - theta4), Izz_3 + Izz_4 + Izz_5 + m3*r3^2 + m4*r4^2 + m5*r4^2 + m5*r5^2 + 2*m5*r4*r5*cos(theta5), - Izz_4 - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - 2*m5*r4*r5*cos(theta5), m5*r5^2 + m5*r4*cos(theta5)*r5 + Izz_5];
M(4,:) = [ L1*m4*r4*cos(theta2 + theta3 - theta4) - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - Izz_4 + L1*m5*r4*cos(theta2 + theta3 - theta4) + L2*m5*r5*cos(theta3 - theta4 + theta5) - m4*r1*r4*cos(theta2 + theta3 - theta4) - m5*r1*r4*cos(theta2 + theta3 - theta4) - m5*r2*r5*cos(theta3 - theta4 + theta5) + L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) - 2*m5*r4*r5*cos(theta5) + L2*m4*r4*cos(theta3 - theta4) + L2*m5*r4*cos(theta3 - theta4) - m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5) - m4*r2*r4*cos(theta3 - theta4) - m5*r2*r4*cos(theta3 - theta4), L2*m5*r5*cos(theta3 - theta4 + theta5) - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - Izz_4 - m5*r2*r5*cos(theta3 - theta4 + theta5) - 2*m5*r4*r5*cos(theta5) + L2*m4*r4*cos(theta3 - theta4) + L2*m5*r4*cos(theta3 - theta4) - m4*r2*r4*cos(theta3 - theta4) - m5*r2*r4*cos(theta3 - theta4), - Izz_4 - Izz_5 - m4*r4^2 - m5*r4^2 - m5*r5^2 - 2*m5*r4*r5*cos(theta5), Izz_4 + Izz_5 + m4*r4^2 + m5*r4^2 + m5*r5^2 + 2*m5*r4*r5*cos(theta5), - m5*r5^2 - m5*r4*cos(theta5)*r5 - Izz_5];
M(5,:) = [ Izz_5 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) - L1*m5*r5*cos(theta2 + theta3 - theta4 + theta5) + m5*r4*r5*cos(theta5) + m5*r1*r5*cos(theta2 + theta3 - theta4 + theta5), Izz_5 + m5*r5^2 - L2*m5*r5*cos(theta3 - theta4 + theta5) + m5*r2*r5*cos(theta3 - theta4 + theta5) + m5*r4*r5*cos(theta5), m5*r5^2 + m5*r4*cos(theta5)*r5 + Izz_5, - m5*r5^2 - m5*r4*cos(theta5)*r5 - Izz_5, m5*r5^2 + Izz_5];

RHS(1,1) = u1 - g*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) - sin(theta1)*(L1 - r1) + r4*sin(theta1 + theta2 + theta3 - theta4)) + g*m4*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1) - r4*sin(theta1 + theta2 + theta3 - theta4)) + g*m2*(sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1)) + g*m3*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2) + sin(theta1)*(L1 - r1)) + g*m1*sin(theta1)*(L1 - r1) + m4*r1*r4*theta2_dot^2*sin(theta2 + theta3 - theta4) + m4*r1*r4*theta3_dot^2*sin(theta2 + theta3 - theta4) + m5*r1*r4*theta2_dot^2*sin(theta2 + theta3 - theta4) + m4*r1*r4*theta4_dot^2*sin(theta2 + theta3 - theta4) + m5*r1*r4*theta3_dot^2*sin(theta2 + theta3 - theta4) + m5*r1*r4*theta4_dot^2*sin(theta2 + theta3 - theta4) + m5*r2*r5*theta3_dot^2*sin(theta3 - theta4 + theta5) + m5*r2*r5*theta4_dot^2*sin(theta3 - theta4 + theta5) + m5*r2*r5*theta5_dot^2*sin(theta3 - theta4 + theta5) + L1*m3*r3*theta2_dot^2*sin(theta2 + theta3) + L1*m3*r3*theta3_dot^2*sin(theta2 + theta3) + L1*L2*m2*theta2_dot^2*sin(theta2) + L1*L2*m3*theta2_dot^2*sin(theta2) + L1*L2*m4*theta2_dot^2*sin(theta2) + L1*L2*m5*theta2_dot^2*sin(theta2) - m3*r1*r3*theta2_dot^2*sin(theta2 + theta3) - m3*r1*r3*theta3_dot^2*sin(theta2 + theta3) - L1*m2*r2*theta2_dot^2*sin(theta2) - L2*m2*r1*theta2_dot^2*sin(theta2) - L1*m3*r2*theta2_dot^2*sin(theta2) - L2*m3*r1*theta2_dot^2*sin(theta2) - L1*m4*r2*theta2_dot^2*sin(theta2) - L2*m4*r1*theta2_dot^2*sin(theta2) - L1*m5*r2*theta2_dot^2*sin(theta2) - L2*m5*r1*theta2_dot^2*sin(theta2) + L2*m3*r3*theta3_dot^2*sin(theta3) - L1*m5*r5*theta2_dot^2*sin(theta2 + theta3 - theta4 + theta5) - L1*m5*r5*theta3_dot^2*sin(theta2 + theta3 - theta4 + theta5) - L1*m5*r5*theta4_dot^2*sin(theta2 + theta3 - theta4 + theta5) - L1*m5*r5*theta5_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m2*r1*r2*theta2_dot^2*sin(theta2) + m3*r1*r2*theta2_dot^2*sin(theta2) + m4*r1*r2*theta2_dot^2*sin(theta2) + m5*r1*r2*theta2_dot^2*sin(theta2) - m3*r2*r3*theta3_dot^2*sin(theta3) + m5*r4*r5*theta5_dot^2*sin(theta5) - L2*m4*r4*theta3_dot^2*sin(theta3 - theta4) - L2*m4*r4*theta4_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta3_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta4_dot^2*sin(theta3 - theta4) + m5*r1*r5*theta2_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m5*r1*r5*theta3_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m5*r1*r5*theta4_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m5*r1*r5*theta5_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m4*r2*r4*theta3_dot^2*sin(theta3 - theta4) + m4*r2*r4*theta4_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta3_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta4_dot^2*sin(theta3 - theta4) - L1*m4*r4*theta2_dot^2*sin(theta2 + theta3 - theta4) - L1*m4*r4*theta3_dot^2*sin(theta2 + theta3 - theta4) - L1*m5*r4*theta2_dot^2*sin(theta2 + theta3 - theta4) - L1*m4*r4*theta4_dot^2*sin(theta2 + theta3 - theta4) - L1*m5*r4*theta3_dot^2*sin(theta2 + theta3 - theta4) - L1*m5*r4*theta4_dot^2*sin(theta2 + theta3 - theta4) - L2*m5*r5*theta3_dot^2*sin(theta3 - theta4 + theta5) - L2*m5*r5*theta4_dot^2*sin(theta3 - theta4 + theta5) - L2*m5*r5*theta5_dot^2*sin(theta3 - theta4 + theta5) - 2*L1*m4*r4*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4) - 2*L1*m4*r4*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4) - 2*L1*m5*r4*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4) + 2*L1*m4*r4*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*L1*m4*r4*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4) - 2*L1*m5*r4*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4) + 2*L1*m4*r4*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*L1*m5*r4*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*L1*m5*r4*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4) + 2*L1*m4*r4*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*L1*m5*r4*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*L1*m5*r4*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*L2*m5*r5*theta1_dot*theta3_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta1_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta2_dot*theta3_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta1_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta2_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta2_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta3_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta3_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta4_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*m4*r1*r4*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4) + 2*m4*r1*r4*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4) + 2*m5*r1*r4*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4) - 2*m4*r1*r4*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*m4*r1*r4*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4) + 2*m5*r1*r4*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4) - 2*m4*r1*r4*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*m5*r1*r4*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*m5*r1*r4*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4) - 2*m4*r1*r4*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*m5*r1*r4*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4) - 2*m5*r1*r4*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4) + 2*m5*r2*r5*theta1_dot*theta3_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta1_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta2_dot*theta3_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta1_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta2_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta2_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta3_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta3_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta4_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L1*m3*r3*theta1_dot*theta2_dot*sin(theta2 + theta3) + 2*L1*m3*r3*theta1_dot*theta3_dot*sin(theta2 + theta3) + 2*L1*m3*r3*theta2_dot*theta3_dot*sin(theta2 + theta3) + 2*L1*L2*m2*theta1_dot*theta2_dot*sin(theta2) + 2*L1*L2*m3*theta1_dot*theta2_dot*sin(theta2) + 2*L1*L2*m4*theta1_dot*theta2_dot*sin(theta2) + 2*L1*L2*m5*theta1_dot*theta2_dot*sin(theta2) - 2*m3*r1*r3*theta1_dot*theta2_dot*sin(theta2 + theta3) - 2*m3*r1*r3*theta1_dot*theta3_dot*sin(theta2 + theta3) - 2*m3*r1*r3*theta2_dot*theta3_dot*sin(theta2 + theta3) - 2*L1*m2*r2*theta1_dot*theta2_dot*sin(theta2) - 2*L2*m2*r1*theta1_dot*theta2_dot*sin(theta2) - 2*L1*m3*r2*theta1_dot*theta2_dot*sin(theta2) - 2*L2*m3*r1*theta1_dot*theta2_dot*sin(theta2) - 2*L1*m4*r2*theta1_dot*theta2_dot*sin(theta2) - 2*L2*m4*r1*theta1_dot*theta2_dot*sin(theta2) - 2*L1*m5*r2*theta1_dot*theta2_dot*sin(theta2) - 2*L2*m5*r1*theta1_dot*theta2_dot*sin(theta2) + 2*L2*m3*r3*theta1_dot*theta3_dot*sin(theta3) + 2*L2*m3*r3*theta2_dot*theta3_dot*sin(theta3) - 2*L1*m5*r5*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*L1*m5*r5*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*L1*m5*r5*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*L1*m5*r5*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*L1*m5*r5*theta1_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*L1*m5*r5*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*L1*m5*r5*theta2_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*L1*m5*r5*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*L1*m5*r5*theta3_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*L1*m5*r5*theta4_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m2*r1*r2*theta1_dot*theta2_dot*sin(theta2) + 2*m3*r1*r2*theta1_dot*theta2_dot*sin(theta2) + 2*m4*r1*r2*theta1_dot*theta2_dot*sin(theta2) + 2*m5*r1*r2*theta1_dot*theta2_dot*sin(theta2) - 2*m3*r2*r3*theta1_dot*theta3_dot*sin(theta3) - 2*m3*r2*r3*theta2_dot*theta3_dot*sin(theta3) + 2*m5*r4*r5*theta1_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta2_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta3_dot*theta5_dot*sin(theta5) - 2*m5*r4*r5*theta4_dot*theta5_dot*sin(theta5) - 2*L2*m4*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) - 2*L2*m4*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) - 2*L2*m5*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) - 2*L2*m5*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) + 2*m5*r1*r5*theta1_dot*theta2_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m5*r1*r5*theta1_dot*theta3_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*m5*r1*r5*theta1_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m5*r1*r5*theta2_dot*theta3_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m5*r1*r5*theta1_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*m5*r1*r5*theta2_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m5*r1*r5*theta2_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*m5*r1*r5*theta3_dot*theta4_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m5*r1*r5*theta3_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) - 2*m5*r1*r5*theta4_dot*theta5_dot*sin(theta2 + theta3 - theta4 + theta5) + 2*m4*r2*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) + 2*m4*r2*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) + 2*m5*r2*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) + 2*m5*r2*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta3_dot*theta4_dot*sin(theta3 - theta4);
RHS(2,1) = u2 + g*m4*(sin(theta1 + theta2)*(L2 - r2) - r4*sin(theta1 + theta2 + theta3 - theta4)) + g*m3*(r3*sin(theta1 + theta2 + theta3) + sin(theta1 + theta2)*(L2 - r2)) - g*m5*(r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - sin(theta1 + theta2)*(L2 - r2) + r4*sin(theta1 + theta2 + theta3 - theta4)) + g*m2*sin(theta1 + theta2)*(L2 - r2) - m4*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - m5*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + m5*r2*r5*theta3_dot^2*sin(theta3 - theta4 + theta5) + m5*r2*r5*theta4_dot^2*sin(theta3 - theta4 + theta5) + m5*r2*r5*theta5_dot^2*sin(theta3 - theta4 + theta5) - L1*m3*r3*theta1_dot^2*sin(theta2 + theta3) - L1*L2*m2*theta1_dot^2*sin(theta2) - L1*L2*m3*theta1_dot^2*sin(theta2) - L1*L2*m4*theta1_dot^2*sin(theta2) - L1*L2*m5*theta1_dot^2*sin(theta2) + m3*r1*r3*theta1_dot^2*sin(theta2 + theta3) + L1*m2*r2*theta1_dot^2*sin(theta2) + L2*m2*r1*theta1_dot^2*sin(theta2) + L1*m3*r2*theta1_dot^2*sin(theta2) + L2*m3*r1*theta1_dot^2*sin(theta2) + L1*m4*r2*theta1_dot^2*sin(theta2) + L2*m4*r1*theta1_dot^2*sin(theta2) + L1*m5*r2*theta1_dot^2*sin(theta2) + L2*m5*r1*theta1_dot^2*sin(theta2) + L2*m3*r3*theta3_dot^2*sin(theta3) + L1*m5*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) - m2*r1*r2*theta1_dot^2*sin(theta2) - m3*r1*r2*theta1_dot^2*sin(theta2) - m4*r1*r2*theta1_dot^2*sin(theta2) - m5*r1*r2*theta1_dot^2*sin(theta2) - m3*r2*r3*theta3_dot^2*sin(theta3) + m5*r4*r5*theta5_dot^2*sin(theta5) - L2*m4*r4*theta3_dot^2*sin(theta3 - theta4) - L2*m4*r4*theta4_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta3_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta4_dot^2*sin(theta3 - theta4) - m5*r1*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m4*r2*r4*theta3_dot^2*sin(theta3 - theta4) + m4*r2*r4*theta4_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta3_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta4_dot^2*sin(theta3 - theta4) + L1*m4*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + L1*m5*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - L2*m5*r5*theta3_dot^2*sin(theta3 - theta4 + theta5) - L2*m5*r5*theta4_dot^2*sin(theta3 - theta4 + theta5) - L2*m5*r5*theta5_dot^2*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta1_dot*theta3_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta1_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta2_dot*theta3_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta1_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta2_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta2_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta3_dot*theta4_dot*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta3_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta4_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta1_dot*theta3_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta1_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta2_dot*theta3_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta1_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta2_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta2_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta3_dot*theta4_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta3_dot*theta5_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta4_dot*theta5_dot*sin(theta3 - theta4 + theta5) + 2*L2*m3*r3*theta1_dot*theta3_dot*sin(theta3) + 2*L2*m3*r3*theta2_dot*theta3_dot*sin(theta3) - 2*m3*r2*r3*theta1_dot*theta3_dot*sin(theta3) - 2*m3*r2*r3*theta2_dot*theta3_dot*sin(theta3) + 2*m5*r4*r5*theta1_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta2_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta3_dot*theta5_dot*sin(theta5) - 2*m5*r4*r5*theta4_dot*theta5_dot*sin(theta5) - 2*L2*m4*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) - 2*L2*m4*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) - 2*L2*m5*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) - 2*L2*m5*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) + 2*L2*m4*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) + 2*m4*r2*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) + 2*m4*r2*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) + 2*m5*r2*r4*theta1_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta1_dot*theta4_dot*sin(theta3 - theta4) + 2*m5*r2*r4*theta2_dot*theta3_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta3_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta2_dot*theta4_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta3_dot*theta4_dot*sin(theta3 - theta4);
RHS(3,1) = u3 - g*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - g*m4*r4*sin(theta1 + theta2 + theta3 - theta4) - g*m5*r4*sin(theta1 + theta2 + theta3 - theta4) + g*m3*r3*sin(theta1 + theta2 + theta3) - m4*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - m5*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - m5*r2*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) - m5*r2*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) - L1*m3*r3*theta1_dot^2*sin(theta2 + theta3) + m3*r1*r3*theta1_dot^2*sin(theta2 + theta3) - L2*m3*r3*theta1_dot^2*sin(theta3) - L2*m3*r3*theta2_dot^2*sin(theta3) + L1*m5*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m3*r2*r3*theta1_dot^2*sin(theta3) + m3*r2*r3*theta2_dot^2*sin(theta3) + m5*r4*r5*theta5_dot^2*sin(theta5) + L2*m4*r4*theta1_dot^2*sin(theta3 - theta4) + L2*m4*r4*theta2_dot^2*sin(theta3 - theta4) + L2*m5*r4*theta1_dot^2*sin(theta3 - theta4) + L2*m5*r4*theta2_dot^2*sin(theta3 - theta4) - m5*r1*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) - m4*r2*r4*theta1_dot^2*sin(theta3 - theta4) - m4*r2*r4*theta2_dot^2*sin(theta3 - theta4) - m5*r2*r4*theta1_dot^2*sin(theta3 - theta4) - m5*r2*r4*theta2_dot^2*sin(theta3 - theta4) + L1*m4*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + L1*m5*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + L2*m5*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) + L2*m5*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) - 2*L2*m3*r3*theta1_dot*theta2_dot*sin(theta3) + 2*m3*r2*r3*theta1_dot*theta2_dot*sin(theta3) + 2*m5*r4*r5*theta1_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta2_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta3_dot*theta5_dot*sin(theta5) - 2*m5*r4*r5*theta4_dot*theta5_dot*sin(theta5) + 2*L2*m4*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) + 2*L2*m5*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) - 2*m4*r2*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) - 2*m5*r2*r4*theta1_dot*theta2_dot*sin(theta3 - theta4);
RHS(4,:) = u4 + g*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) + g*m4*r4*sin(theta1 + theta2 + theta3 - theta4) + g*m5*r4*sin(theta1 + theta2 + theta3 - theta4) + m4*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + m5*r1*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) + m5*r2*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) + m5*r2*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) - L1*m5*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) - m5*r4*r5*theta5_dot^2*sin(theta5) - L2*m4*r4*theta1_dot^2*sin(theta3 - theta4) - L2*m4*r4*theta2_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta1_dot^2*sin(theta3 - theta4) - L2*m5*r4*theta2_dot^2*sin(theta3 - theta4) + m5*r1*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) + m4*r2*r4*theta1_dot^2*sin(theta3 - theta4) + m4*r2*r4*theta2_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta1_dot^2*sin(theta3 - theta4) + m5*r2*r4*theta2_dot^2*sin(theta3 - theta4) - L1*m4*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - L1*m5*r4*theta1_dot^2*sin(theta2 + theta3 - theta4) - L2*m5*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) - L2*m5*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) - 2*L2*m5*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) + 2*m5*r2*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) - 2*m5*r4*r5*theta1_dot*theta5_dot*sin(theta5) - 2*m5*r4*r5*theta2_dot*theta5_dot*sin(theta5) - 2*m5*r4*r5*theta3_dot*theta5_dot*sin(theta5) + 2*m5*r4*r5*theta4_dot*theta5_dot*sin(theta5) - 2*L2*m4*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) - 2*L2*m5*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) + 2*m4*r2*r4*theta1_dot*theta2_dot*sin(theta3 - theta4) + 2*m5*r2*r4*theta1_dot*theta2_dot*sin(theta3 - theta4);
RHS(5,:) = u5 - g*m5*r5*sin(theta1 + theta2 + theta3 - theta4 + theta5) - m5*r2*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) - m5*r2*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) + L1*m5*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) - m5*r4*r5*theta1_dot^2*sin(theta5) - m5*r4*r5*theta2_dot^2*sin(theta5) - m5*r4*r5*theta3_dot^2*sin(theta5) - m5*r4*r5*theta4_dot^2*sin(theta5) - m5*r1*r5*theta1_dot^2*sin(theta2 + theta3 - theta4 + theta5) + L2*m5*r5*theta1_dot^2*sin(theta3 - theta4 + theta5) + L2*m5*r5*theta2_dot^2*sin(theta3 - theta4 + theta5) + 2*L2*m5*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) - 2*m5*r2*r5*theta1_dot*theta2_dot*sin(theta3 - theta4 + theta5) - 2*m5*r4*r5*theta1_dot*theta2_dot*sin(theta5) - 2*m5*r4*r5*theta1_dot*theta3_dot*sin(theta5) + 2*m5*r4*r5*theta1_dot*theta4_dot*sin(theta5) - 2*m5*r4*r5*theta2_dot*theta3_dot*sin(theta5) + 2*m5*r4*r5*theta2_dot*theta4_dot*sin(theta5) + 2*m5*r4*r5*theta3_dot*theta4_dot*sin(theta5);

% ODE and path constraints 
ceq = collocate({M(1,:)*dot(qd) == RHS(1); 
    M(2,:)*dot(qd) == RHS(2);
    M(3,:)*dot(qd) == RHS(3);
    M(4,:)*dot(qd) == RHS(4);
    M(5,:)*dot(qd) == RHS(5);
    qd == dot(q)});

% Function to be optimized
objective = integrate(u1*u1 + u2*u2 + u3*u3 + u4*u4 + u5*u5);
%objective = tf;

% Solve the problem
options = struct;
options.name = 'FiveLinkHumanoid';
options.solve = 'snopt';
solution = ezsolve(objective,{cbox,cbnd,ceq},x0, options);

% grabbing solution
opt.tf = subs(tf,solution);
opt.ti  = subs(icollocate(t),solution);
opt.tc = subs(collocate(t),solution);
opt.q = subs(icollocate(q),solution);
opt.qd = subs(icollocate(qd),solution);
opt.qd = subs(icollocate(qd),solution);
opt.u = subs(collocate(u),solution); 
opt.objective = subs(objective,solution);

for i = 1:length(opt.ti)
    [opt.pm(:,:,i), opt.pcm(:,:,i)] = computeFK(opt.q(i,:));
    opt.xcm(i) = computeCM(opt.pcm(1,:,i)); % plot Center of Mass in x-position
end

% plotting joint angles 
plot(opt.ti,opt.q);
legend('x1','x2','x3','x4','x5','Location','northeastoutside')
title('Criteria: COM - Objective Function: Minimum Energy')
grid
% plotting joint velocities
figure
plot(opt.ti,opt.qd);
legend('x6','x7','x8','x9','x10','Location','northeastoutside')
title('Criteria: COM - Objective Function:  Minimum Energy')
grid
% plotting center of mass
figure
plot(opt.ti,opt.xcm');
grid
title('Criteria: COM - Objective Function:  Minimum Energy')
legend('xCOM','Location','northeastoutside')
% plotting torque
figure
plot(opt.tc,opt.u);
grid
title('Criteria: COM - Objective Function:  Minimum Energy')
legend('u1','u2','u3','u4','u5','Location','northeastoutside')

for i = 1:length(opt.tc)
    zmpplot(i) = computeZMP(opt.q(i,:),opt.qd(i,:),opt.u(i,:));
end
% plotting zmp
figure
plot(opt.tc,zmpplot);
grid
title('Criteria: COM - Objective Function: Minimum Energy')
legend('xZMP','Location','northeastoutside')



figure
stickFigure(opt);

save('ZMP_minEnergy');
% figure
% filme= simulate(opt);
% movie2avi(filme,'Hum_movie')

